#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <std_msgs/msg/string.hpp>

class VideoDetNode : public rclcpp::Node
{
public:
  VideoDetNode() : Node("video_det_node")
  {
    // 声明参数
    this->declare_parameter("show_video", true);
    this->declare_parameter("save_video", false);
    this->declare_parameter("output_path", "output_video.avi");
    
    // 获取参数
    show_video_ = this->get_parameter("show_video").as_bool();
    save_video_ = this->get_parameter("save_video").as_bool();
    output_path_ = this->get_parameter("output_path").as_string();
    
    // 创建订阅者 - 订阅video_det话题
    image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
      "video_det", 10, std::bind(&VideoDetNode::image_callback, this, std::placeholders::_1)
    );
    
    // 创建发布者 - 发布检测结果
    result_pub_ = this->create_publisher<std_msgs::msg::String>("detection_result", 10);
    
    // 初始化视频写入器
    if (save_video_) {
      init_video_writer();
    }
    
    // 创建定时器用于状态报告
    status_timer_ = this->create_wall_timer(
      std::chrono::seconds(5),
      std::bind(&VideoDetNode::status_callback, this)
    );
    
    RCLCPP_INFO(this->get_logger(), "Video detection node started successfully!");
    RCLCPP_INFO(this->get_logger(), "Subscribing to topic: video_det");
    RCLCPP_INFO(this->get_logger(), "Show video: %s", show_video_ ? "true" : "false");
    RCLCPP_INFO(this->get_logger(), "Save video: %s", save_video_ ? "true" : "false");
    
    frame_count_ = 0;
    last_fps_time_ = this->now();
  }
  
  ~VideoDetNode()
  {
    if (video_writer_.isOpened()) {
      video_writer_.release();
    }
    if (show_video_) {
      cv::destroyAllWindows();
    }
  }

private:
  void init_video_writer()
  {
    int fourcc = cv::VideoWriter::fourcc('X', 'V', 'I', 'D');
    video_writer_.open(output_path_, fourcc, 30.0, cv::Size(640, 480), true);
    
    if (!video_writer_.isOpened()) {
      RCLCPP_ERROR(this->get_logger(), "Failed to open video writer for: %s", output_path_.c_str());
      save_video_ = false;
    } else {
      RCLCPP_INFO(this->get_logger(), "Video writer initialized: %s", output_path_.c_str());
    }
  }
  
  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    try {
      // 转换ROS图像消息为OpenCV格式
      cv::Mat frame = cv_bridge::toCvShare(msg, "bgr8")->image;
      
      if (frame.empty()) {
        RCLCPP_WARN(this->get_logger(), "Received empty frame");
        return;
      }
      
      // 处理帧
      cv::Mat processed_frame = process_frame(frame);
      
      // 保存视频
      if (save_video_ && video_writer_.isOpened()) {
        video_writer_ << processed_frame;
      }
      
      // 显示视频
      if (show_video_) {
        cv::imshow("Video Detection", processed_frame);
        cv::waitKey(1);
      }
      
      // 更新统计信息
      frame_count_++;
      auto current_time = this->now();
      if (frame_count_ % 30 == 0) {
        auto duration = (current_time - last_fps_time_).seconds();
        double fps = 30.0 / duration;
        RCLCPP_INFO(this->get_logger(), "Processing frame %d, FPS: %.1f", frame_count_, fps);
        last_fps_time_ = current_time;
      }
      
      // 发布检测结果
      publish_detection_result();
      
    } catch (cv_bridge::Exception& e) {
      RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
    }
  }
  
  cv::Mat process_frame(const cv::Mat& frame)
  {
    cv::Mat processed = frame.clone();
    
    // 在这里添加你的视频检测算法
    // 例如：目标检测、人脸识别、运动检测等
    
    // 示例：添加时间戳和帧计数
    std::string timestamp = std::to_string(this->now().nanoseconds());
    std::string frame_info = "Frame: " + std::to_string(frame_count_) + 
                            " | Time: " + timestamp.substr(0, 10);
    
    cv::putText(processed, frame_info, cv::Point(10, 30), 
                cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 255, 0), 2);
    
    // 示例：简单的边缘检测
    cv::Mat edges;
    cv::Canny(processed, edges, 100, 200);
    
    // 将边缘检测结果叠加到原图上
    cv::Mat edges_colored;
    cv::cvtColor(edges, edges_colored, cv::COLOR_GRAY2BGR);
    cv::addWeighted(processed, 0.8, edges_colored, 0.2, 0, processed);
    
    return processed;
  }
  
  void publish_detection_result()
  {
    auto result_msg = std_msgs::msg::String();
    result_msg.data = "Frame " + std::to_string(frame_count_) + " processed successfully";
    result_pub_->publish(result_msg);
  }
  
  void status_callback()
  {
    RCLCPP_INFO(this->get_logger(), "Status: Processed %d frames", frame_count_);
  }
  
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr result_pub_;
  rclcpp::TimerBase::SharedPtr status_timer_;
  
  cv::VideoWriter video_writer_;
  
  bool show_video_;
  bool save_video_;
  std::string output_path_;
  
  int frame_count_;
  rclcpp::Time last_fps_time_;
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<VideoDetNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
